#include "KinematicSensorVisualisation.h"

using namespace MMM;

KinematicSensorVisualisation::KinematicSensorVisualisation(KinematicSensorPtr sensor, VirtualRobot::RobotPtr robot, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor),
    robot(robot)
{
}

std::string KinematicSensorVisualisation::getType() {
    return sensor->getType();
}

int KinematicSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

void KinematicSensorVisualisation::update(float timestep, float delta) {
     update(sensor->getDerivedMeasurement(timestep, delta));
}

void KinematicSensorVisualisation::update(float timestep) {
     update(sensor->getDerivedMeasurement(timestep));
}

void KinematicSensorVisualisation::update(KinematicSensorMeasurementPtr measurement) {
     if (!measurement) return;

     for (unsigned int i = 0; i < sensor->getJointNames().size(); i++) {
         robot->setJointValue(sensor->getJointNames()[i], measurement->getJointAngles()[i]);
     }
}

void KinematicSensorVisualisation::displayVisualisation(bool display) {
    SensorVisualisation::displayVisualisation(display);
    if (display) update(lastTimestep, lastDelta);
}
